/*
 * Author  : Leo <lipf160627@163.com>
 * Created : 2025-7-27
 * Desc    : Co-mcu using USART to communicate with STM32
 */

#include "co_mcu.hpp"

#include "esp_log.h"

static const char* TAG = "CoMcu";

static const int RX_BUF_SIZE = 128; // 必须是128的倍数

CoMcu::CoMcu(uart_port_t uartNum, gpio_num_t txPin, gpio_num_t rxPin, int baudRate)
    :m_uartNum(uartNum), m_pinTx(txPin), m_pinRx(rxPin), m_baudRate(baudRate)
{
    this->init();
}

void CoMcu::init()
{
    const uart_config_t uart_config = {
        .baud_rate  = this->m_baudRate,
        .data_bits  = UART_DATA_8_BITS,
        .parity     = UART_PARITY_DISABLE,
        .stop_bits  = UART_STOP_BITS_1,
        .flow_ctrl  = UART_HW_FLOWCTRL_DISABLE,
        .source_clk = UART_SCLK_DEFAULT,
    };
    // We won't use a buffer for sending data.
    ESP_ERROR_CHECK(uart_driver_install(this->m_uartNum, RX_BUF_SIZE * 2, 0, 0, NULL, 0));
    ESP_ERROR_CHECK(uart_param_config(this->m_uartNum, &uart_config));
    ESP_ERROR_CHECK(uart_set_pin(this->m_uartNum, this->m_pinTx, this->m_pinRx, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));

    ESP_LOGI(TAG, "Initialized: UART TX=%d, RX=%d, baudRate=%d", m_pinTx, m_pinRx, m_baudRate);
}

bool CoMcu::sendAndWaitForResponse(uint8_t cmd) {
    this->generateChecksum();
    if (uart_write_bytes(this->m_uartNum, this->m_sendBuf.data(), CO_MCU_REQUEST_SIZE) != CO_MCU_REQUEST_SIZE) {
        return false;
    }

    if (CO_MCU_RESPONSE_SIZE ==
        uart_read_bytes(this->m_uartNum, this->m_recvBuf.data(), this->m_recvBuf.size(), pdMS_TO_TICKS(100))) {
        if (this->m_recvBuf[0] == 0xAA && this->m_recvBuf[1] == 0x55 &&
            this->m_recvBuf[2] == 0xE0 && this->m_recvBuf[3] == cmd &&
            this->verifyChecksum()) {
            return CO_MCU_ACK_SUCCESS == this->m_recvBuf[3];
        }
    }
    return false;
}

bool CoMcu::forward(uint8_t distance, uint8_t speed)
{
    this->m_sendBuf = {0xAA, 0x55, CO_MCU_CMD_FORWARD, distance, speed, 0, 0, 0};
    return this->sendAndWaitForResponse(CO_MCU_CMD_FORWARD);
}

bool CoMcu::back(uint8_t distance, uint8_t speed)
{
    this->m_sendBuf = {0xAA, 0x55, CO_MCU_CMD_BACK, distance, speed, 0, 0, 0};
    return this->sendAndWaitForResponse(CO_MCU_CMD_BACK);
}

bool CoMcu::turnLeft(uint8_t angle, uint8_t speed)
{
    this->m_sendBuf = {0xAA, 0x55, CO_MCU_CMD_TURN_LEFT, angle, speed, 0, 0, 0};
    return this->sendAndWaitForResponse(CO_MCU_CMD_TURN_LEFT);
}

bool CoMcu::turnRight(uint8_t angle, uint8_t speed)
{
    this->m_sendBuf = {0xAA, 0x55, CO_MCU_CMD_TURN_RIGHT, angle, speed, 0, 0, 0};
    return this->sendAndWaitForResponse(CO_MCU_CMD_TURN_RIGHT);
}

bool CoMcu::stop()
{
    this->m_sendBuf = {0xAA, 0x55, CO_MCU_CMD_STOP, 0, 0, 0, 0, 0};
    return this->sendAndWaitForResponse(CO_MCU_CMD_STOP);
}

bool CoMcu::terminate()
{
    this->m_sendBuf = {0xAA, 0x55, CO_MCU_CMD_TERMINATE, 0, 0, 0, 0, 0};
    return this->sendAndWaitForResponse(CO_MCU_CMD_TERMINATE);
}

bool CoMcu::verifyChecksum()
{
    uint8_t checksum = 0;

    for (int i = 0; i < CO_MCU_REQUEST_SIZE - 1; i++) {
        checksum ^= this->m_recvBuf[i];
    }
    return checksum == this->m_recvBuf[CO_MCU_REQUEST_SIZE - 1];
}

uint8_t CoMcu::generateChecksum()
{
    uint8_t checksum = 0;

    for (int i = 0; i < CO_MCU_REQUEST_SIZE - 1; i++) {
        checksum ^= this->m_sendBuf[i];
    }
    return checksum;
}
